#include "motor.h"
#include "tim.h"
#include "gpio.h"
#include "stm32f1xx_hal.h"
#define L1 HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_3)==GPIO_PIN_SET
#define L1L HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_3)==GPIO_PIN_RESET
#define L2 HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_12)==GPIO_PIN_SET
#define L2L HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_12)==GPIO_PIN_RESET
#define R1 HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_5)==GPIO_PIN_SET
#define R1L HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_5)==GPIO_PIN_RESET
#define R2 HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_4)==GPIO_PIN_SET
#define R2L HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_4)==GPIO_PIN_RESET
int i=0;
enum motorstate{
  forward,
	left,
	right,
	stop
};
enum motorstate currentState =stop;
void sensor(void){
		if(i==0x00000007){Stop();} 
		  if (L1&&L2L&&R1&&R2L)
       {
        Forward();
			  currentState=forward;
       }
		 else if(L1&&L2L&&R1&&R2)
			 {
		    B_Left();
			  currentState=forward;
			 }
		 else if(L1&&L2&&R1&&R2L)
			 {
		    B_Right();
			  currentState=forward;
		   }
		 else if(L1&&L2&&R1L&&R2)
			 {
	      B_Right();
		    currentState=right;
		   }
		 else if(L1L&&L2&&R1&&R2)
			 {
         B_Left();
			  currentState=left;
		   }
		 else if(L1L&&L2L&&R1L&&R2L)
			 {
				 Stop();
				 HAL_Delay(500);  
				 i++;
				      if(i==0x00000002){HAL_Delay(500);Stop();HAL_Delay(500);}
				      else if(i==0x00000003){S_Left(); HAL_Delay(820);Stop();}
				      else if(i==0x00000004){S_Right();HAL_Delay(820);Stop();}
				      else if(i==0x00000005){Forward();HAL_Delay(400);B_Left();HAL_Delay(1000);}
		   }
			  if(L1&&L2&&R1&&R2)
			 {
		           if(currentState==forward){Forward();}
	       	     else if(currentState==left){B_Left();}
		           else if(currentState==right){B_Right();}
	
			 }
     else if(L1L&&L2L&&R1&&R2L){Forward();currentState=forward;}
     else if(L1&&L2L&&R1L&&R2L){Forward();currentState=forward;}
     else if(L1L&&L2L&&R1&&R2){Forward();currentState=forward;}
		 else if(L1&&L2&&R1L&&R2L){Forward();currentState=forward;}
		
}






























